Novel insights into the impact of graph structure on SLAM

© 2014 IEEE. SLAM can be viewed as an estimation problem over graphs. It is well known that the topology of each dataset affects the quality of the corresponding optimal estimate. In this paper we present a formal analysis of the impact of graph structure on the reliability of the maximum likelihood estimator. In particular, we show that the number of spanning trees in the graph is closely related to the D-optimality criterion in experimental design. We also reveal that in a special class of linear-Gaussian estimation problems over graphs, the algebraic connectivity is related to the E-optimality design criterion. Furthermore, we explain how the average node degree of the graph is related to the ratio between the minimum of negative log-likelihood achievable and its value at the ground truth. These novel insights give us a deeper understanding of the SLAM problem. Finally we discuss two important applications of our analysis in active measurement selection and graph pruning. The results obtained from simulations and experiments on real data confirm our theoretical findings

Tree-connectivity: Evaluating the graphical structure of SLAM

© 2016 IEEE. Simultaneous localization and mapping (SLAM) in robotics, and a number of related problems that arise in sensor networks are instances of estimation problems over weighted graphs. This paper studies the relation between the graphical representation of such problems and estimationtheoretic concepts such as the Cramér-Rao lower bound (CRLB) and D-optimality. We prove that the weighted number of spanning trees, as a graph connectivity metric, is closely related to the determinant of CRLB. This metric can be efficiently computed for large graphs by exploiting the sparse structure of underlying estimation problems. Our analysis is validated using experiments with publicly available pose-graph SLAM datasets

Towards a reliable SLAM back-end

In the state-of-the-art approaches to SLAM, the problem is often formulated as a non-linear least squares. SLAM back-ends often employ iterative methods such as Gauss-Newton or Levenberg-Marquardt to solve that problem. In general, there is no guarantee on the global convergence of these methods. The back-end might get trapped into a local minimum or even diverge depending on how good the initial estimate is. Due to the large noise in odometry data, it is not wise to rely on dead reckoning for obtaining an initial guess, especially in long trajectories. In this paper we demonstrate how M-estimation can be used as a bootstrapping technique to obtain a reliable initial guess. We show that this initial guess is more likely to be in the basin of attraction of the global minimum than existing bootstrapping methods. As the main contribution of this paper, we present new insights about the similarities between robustness against outliers and robustness against a bad initial guess. Through simulations and experiments on real data, we substantiate the reliability of our proposed method. © 2013 IEEE

Exploiting the separable structure of SLAM

© 2015, MIT Press Journals. All rights reserved. In this paper we point out an overlooked structure of SLAM that distinguishes it from a generic nonlinear least squares problem. The measurement function in most common forms of SLAM is linear with respect to robot and features' positions. Therefore, given an estimate for robot orientation, the conditionally optimal estimate for the rest of state variables can be easily obtained by solving a sparse linear-Gaussian estimation problem. We propose an algorithm to exploit this intrinsic property of SLAM by stripping the problem down to its nonlinear core, while maintaining its natural sparsity. Our algorithm can be used together with any Newton-based iterative solver and is applicable to 2D/3D pose-graph and feature-based problems. Our results suggest that iteratively solving the nonlinear core of SLAM leads to a fast and reliable convergence as compared to the state-of-the-art back-ends

Designing Sparse Reliable Pose-Graph SLAM: A Graph-Theoretic Approach

In this paper, we aim to design sparse D-optimal (determinantoptimal) pose-graph SLAM problems through the synthesis of sparse graphs with the maximum weighted number of spanning trees. Characterizing graphs with the maximum number of spanning trees is an open problem in general. To tackle this problem, several new theoretical results are established in this paper, including the monotone log-submodularity of the weighted number of spanning trees. By exploiting these structures, we design a complementary pair of near-optimal efficient approximation algorithms with provable guarantees. Our theoretical results are validated using random graphs and a publicly available pose-graph SLAM dataset.Comment: WAFR 201

Dimensionality reduction for point feature SLAM problems with spherical covariance matrices

© 2014 Elsevier Ltd. All rights reserved. The main contribution of this paper is the dimensionality reduction for multiple-step 2D point feature based Simultaneous Localization and Mapping (SLAM), which is an extension of our previous work on one-step SLAM (Wang et al.; 2013). It has been proved that SLAM with multiple robot poses and a number of point feature positions as variables is equivalent to an optimization problem with only the robot orientations as variables, when the associated uncertainties can be described using spherical covariance matrices. This reduces the dimension of original problem from 3m+2n to m only (where m is the number of poses and n is the number of features). The optimization problem after dimensionality reduction can be solved numerically using the unconstrained optimization algorithms. While dimensionality reduction may not provide computational saving for all nonlinear optimization problems, for some SLAM problems we can achieve benefits such as improvement on time consumption and convergence. For the special case of two-step SLAM when the orientation information from odometry is not incorporated, an algorithm that can guarantee to obtain the globally optimal solution (in the maximum likelihood sense) is derived. Simulation and experimental datasets are used to verify the equivalence between the reduced nonlinear optimization problem and the original full optimization problem, as well as the proposed new algorithm for obtaining the globally optimal solution for two-step SLAM

A sparse separable SLAM back-end

© 2004-2012 IEEE. We propose a scalable algorithm to take advantage of the separable structure of simultaneous localization and mapping (SLAM). Separability is an overlooked structure of SLAM that distinguishes it from a generic nonlinear least-squares problem. The standard relative-pose and relative-position measurement models in SLAM are affine with respect to robot and features' positions. Therefore, given an estimate for robot orientation, the conditionally optimal estimate for the rest of the state variables can be easily computed by solving a sparse linear least-squares problem. We propose an algorithm to exploit this intrinsic property of SLAM by stripping the problem down to its nonlinear core, while maintaining its natural sparsity. Our algorithm can be used in conjunction with any Newton-based solver and is applicable to 2-D/3-D pose-graph and feature-based SLAM. Our results suggest that iteratively solving the nonlinear core of SLAM leads to a fast and reliable convergence as compared to the state-of-the-art sparse back-ends

Monte Carlo sampling of non-Gaussian proposal distribution in feature-based RBPF-SLAM

Particle filters are widely used in mobile robot localization and mapping. It is well-known that choosing an appropriate proposal distribution plays a crucial role in the success of particle filters. The proposal distribution conditioned on the most recent observation, known as the optimal proposal distribution (OPD), increases the number of effective particles and limits the degeneracy of filter. Conventionally, the OPD is approximated by a Gaussian distribution, which can lead to failure if the true distribution is highly non-Gaussian. In this paper we propose two novel solutions to the problem of feature-based SLAM, through Monte Carlo approximation of the OPD which show superior results in terms of mean squared error (MSE) and number of effective samples. The proposed methods are capable of describing non-Gaussian OPD and dealing with nonlinear models. Simulation and experimental results in large-scale environments show that the new algorithms outperform the aforementioned conventional methods

Reliable Graphs for SLAM

© The Author(s) 2019. Estimation-over-graphs (EoG) is a class of estimation problems that admit a natural graphical representation. Several key problems in robotics and sensor networks, including sensor network localization, synchronization over a group, and simultaneous localization and mapping (SLAM) fall into this category. We pursue two main goals in this work. First, we aim to characterize the impact of the graphical structure of SLAM and related problems on estimation reliability. We draw connections between several notions of graph connectivity and various properties of the underlying estimation problem. In particular, we establish results on the impact of the weighted number of spanning trees on the D-optimality criterion in 2D SLAM. These results enable agents to evaluate estimation reliability based only on the graphical representation of the EoG problem. We then use our findings and study the problem of designing sparse SLAM problems that lead to reliable maximum likelihood estimates through the synthesis of sparse graphs with the maximum weighted tree connectivity. Characterizing graphs with the maximum number of spanning trees is an open problem in general. To tackle this problem, we establish several new theoretical results, including the monotone log-submodularity of the weighted number of spanning trees. We exploit these structures and design a complementary greedy–convex pair of efficient approximation algorithms with provable guarantees. The proposed synthesis framework is applied to various forms of the measurement selection problem in resource-constrained SLAM. Our algorithms and theoretical findings are validated using random graphs, existing and new synthetic SLAM benchmarks, and publicly available real pose-graph SLAM datasets